#include "magtet.h"

void magtet::read_tet(std::string tet_name)
{
	gctl::read_Tetgen_node(tet_name, node_);
	gctl::read_Tetgen_element(tet_name, ele_, node_);

	node_num_ = node_.size();
	ele_num_ = ele_.size();
	return;
}

void magtet::init_magz(std::string para)
{
	// try to use the para as a file name
	if (access(para.c_str(), F_OK) != -1)
	{
		std::vector<gctl::point3dc> tmp_vec;
		gctl::read_text2vector(para, tmp_vec);

		if (tmp_vec.size() != ele_num_)
		{
			throw gctl::length_error("Invalid magnetization vector size. From magtet::init_magz(...)");
		}

		magz_.resize(tmp_vec);
		gctl::destroy_vector(tmp_vec);
		return;
	}

	// try to use the para in the format <mag_x>,<mag_y>,<mag_z>
	gctl::point3dc tmp_magz;
	tmp_magz.str(para);

	magz_.resize(ele_num_);
	for (int i = 0; i < ele_num_; ++i)
	{
		magz_[i] = tmp_magz;
	}
	return;
}

void magtet::init_site(std::string para)
{
	// try to use the para as a file name
	if (access(para.c_str(), F_OK) != -1)
	{
		std::vector<gctl::point3dc> tmp_vec;
		gctl::read_text2vector(para, tmp_vec);

		site_num_ = tmp_vec.size();
		site_.resize(tmp_vec);
		gctl::destroy_vector(tmp_vec);
		return;
	}

	// try to use the para in the format <xmin>/<xmax>/<ymin>/<ymax>/<z>/<xsize>/<ysize>
	double xmin, xmax, ymin, ymax, z;
	int xsize, ysize;
	gctl::parse_string_to_value(para, '/', xmin, xmax, ymin, ymax, z, xsize, ysize);
	gctl::gridspace(gctl::point3dc(xmin, 0.0, 0.5*z), gctl::point3dc(xmax, 0.0, 0.5*z), 
		gctl::point3dc(0.0, ymin, 0.5*z), gctl::point3dc(0.0, ymax, 0.5*z), xsize, ysize, site_);

	site_num_ = site_.size();
	return;
}

void magtet::write_text(std::string out_name)
{
	std::vector<std::string> opt_info;
	gopt_.get_options(opt_info);

	std::ofstream outfile;
	if (!mag_pot_.empty())
	{
		gctl::open_outfile(outfile, out_name, "_mag_pot.txt");
		for (int i = 0; i < opt_info.size(); ++i)
		{
			outfile << "# " << opt_info[i] << std::endl;
		}
		outfile << "# x(m) y(m) z(m) V" << std::endl;
		for (int i = 0; i < site_num_; ++i)
		{
			outfile << site_[i] << std::setprecision(16) << " " << mag_pot_[i] << std::endl;
		}
		outfile.close();
	}

	if (!mag_grad_.empty())
	{
		gctl::open_outfile(outfile, out_name, "_mag_grad.txt");
		for (int i = 0; i < opt_info.size(); ++i)
		{
			outfile << "# " << opt_info[i] << std::endl;
		}
		outfile << "# x(m) y(m) z(m) Bx(nT) By(nT) Bz(nT)" << std::endl;
		for (int i = 0; i < site_num_; ++i)
		{
			outfile << site_[i] << std::setprecision(16) << " " << mag_grad_[i].x << " " 
				<< mag_grad_[i].y << " " << mag_grad_[i].z << " " << std::endl;
		}
		outfile.close();
	}

	if (!mag_tensor_.empty())
	{
		gctl::open_outfile(outfile, out_name, "_mag_tensor.txt");
		for (int i = 0; i < opt_info.size(); ++i)
		{
			outfile << "# " << opt_info[i] << std::endl;
		}
		outfile << "# x(m) y(m) z(m) Txx(nT/m) Txy(nT/m) Txz(nT/m) Tyx(nT/m) Tyy(nT/m) Tyz(nT/m) Tzx(nT/m) Tzy(nT/m) Tzz(nT/m)" << std::endl;
		for (int i = 0; i < site_num_; ++i)
		{
			outfile << site_[i] << std::setprecision(16) << " " << mag_tensor_[i].at(0,0) << " " 
				<< mag_tensor_[i].at(0,1) << " " << mag_tensor_[i].at(0,2) << " " 
				<< mag_tensor_[i].at(1,0) << " " << mag_tensor_[i].at(1,1) << " " 
				<< mag_tensor_[i].at(1,2) << " " << mag_tensor_[i].at(2,0) << " " 
				<< mag_tensor_[i].at(2,1) << " " << mag_tensor_[i].at(2,2) << std::endl;
		}
		outfile.close();
	}
	return;
}

void magtet::cal_tensors()
{
	ele_para_.resize(ele_num_);

	gctl::point3dc v1, v2, v3, nf, ne;
	for (int e = 0; e < ele_num_; ++e)
	{
		ele_[e].att = ele_para_.get(e);

		for (int i = 0; i < 4; ++i)
		{
			v1 = *ele_[e].fget(i, 1) - *ele_[e].fget(i, 0);
			v2 = *ele_[e].fget(i, 2) - *ele_[e].fget(i, 0);
			nf = gctl::cross(v1, v2).normal();
			// The space is declared by the read_magz() function
			ele_[e].att->fnorm[i] = nf;

			for (int j = 0; j < 3; ++j)
			{
				v3 = *ele_[e].fget(i, (j+1)%3) - *ele_[e].fget(i, j);
				ne = gctl::cross(v3, nf).normal();
				ele_[e].att->enorm[j+i*3] = ne;
				ele_[e].att->etang[j+i*3] = gctl::cross(nf, ne);
			}

			ele_[e].att->mag_amp[i] = gctl::dot(magz_[e], ele_[e].att->fnorm[i]);
		}
	}
	return;
}

double magtet::mag_potential(const mag_tetrahedron &tet, const gctl::point3dc &site)
{
	double Rij_minus, Rij_plus, Sij_plus, Sij_minus, Rij0, mij0, wi0;
	double part1, part2, k0, absw, beta;
	gctl::point3dc oi;

	// get attribute pointer
	magtet_para *mpara = tet.att;

	double out_pot = 0.0;
	for (int f = 0; f < 4; ++f)
	{
		k0 = 0.0;
		for (int j = 0; j < 3; ++j)
		{
			Rij_minus = (site - *tet.fget(f, j)).module();
			Rij_plus  = (site - *tet.fget(f, (j+1)%3)).module();

			if (j == 0)
			{
				wi0 = gctl::dot(site - *tet.fget(f, j), mpara->fnorm[f]);
				absw = std::abs(wi0);
			}

			oi = site - wi0*mpara->fnorm[f];
			Sij_minus = gctl::dot(*tet.fget(f, j) - oi, mpara->etang[3*f+j]);
		    Sij_plus  = gctl::dot(*tet.fget(f, (j+1)%3) - oi, mpara->etang[3*f+j]);
		    mij0 = gctl::dot(*tet.fget(f, j) - oi, mpara->enorm[3*f+j]);
		    Rij0 = std::sqrt(wi0*wi0 + mij0*mij0);

			part2 = 0.0;
			if (absw > TOL)
			{
				beta = std::atan((mij0*Sij_plus)/(Rij0*Rij0+absw*Rij_plus))
					- std::atan((mij0*Sij_minus)/(Rij0*Rij0+absw*Rij_minus));
				part2 = absw*beta;
			}

			part1 = 0.0;
			if (std::abs(mij0) > TOL)
			{
				part1 = mij0*std::log((Rij_plus+Sij_plus)/(Rij_minus+Sij_minus));
			}

			k0 += (part1 - part2);
		}

		out_pot += Cm*k0*mpara->mag_amp[f];
	}

	return out_pot;
}

gctl::point3dc magtet::mag_gradient(const mag_tetrahedron &tet, const gctl::point3dc &site)
{
	double Rij_minus, Rij_plus, Sij_plus, Sij_minus, Rij0, mij0, wi0;
	double beta, Aij, sig, absw;
	gctl::point3dc oi, k1, part1, part2;

	// get attribute pointer
	magtet_para *mpara = tet.att;

	gctl::point3dc out_grad(0.0, 0.0, 0.0);
	for (int f = 0; f < 4; ++f)
	{
		k1.set(0.0, 0.0, 0.0);
		for (int j = 0; j < 3; ++j)
		{
			Rij_minus = (site - *tet.fget(f, j)).module();
			Rij_plus  = (site - *tet.fget(f, (j+1)%3)).module();

			if (j == 0)
			{
				wi0 = gctl::dot(site - *tet.fget(f, j), mpara->fnorm[f]);
				sig = gctl::sign(wi0);
				absw = std::abs(wi0);
			}

			oi = site - wi0*mpara->fnorm[f];
			Sij_minus = gctl::dot(*tet.fget(f, j) - oi, mpara->etang[3*f+j]);
			Sij_plus  = gctl::dot(*tet.fget(f, (j+1)%3) - oi, mpara->etang[3*f+j]);
			mij0 = gctl::dot(*tet.fget(f, j) - oi, mpara->enorm[3*f+j]);
			Rij0 = std::sqrt(wi0*wi0 + mij0*mij0);

			part2.set(0.0, 0.0, 0.0);
			if (absw > TOL)
			{
				beta = atan((mij0*Sij_plus)/(Rij0*Rij0 + absw*Rij_plus))
					- atan((mij0*Sij_minus)/(Rij0*Rij0 + absw*Rij_minus));

				part2 = sig*beta*mpara->fnorm[f];
			}

			if (std::abs(Rij0) > TOL)
			{
				Aij = std::log((long double)(Rij_plus+Sij_plus)) - std::log((long double)(Rij_minus+Sij_minus));
			}
			else if (Sij_plus > 0.0 && Sij_minus > 0.0)
			{
				Aij = std::log(Sij_plus) - std::log(Sij_minus);
			}
			else if (Sij_plus < 0.0 && Sij_minus < 0.0)
			{
				Aij = std::log(-1.0*Sij_minus) - std::log(-1.0*Sij_plus);
			}
			else
			{
				throw gctl::runtime_error("Observation site on edge. From magtet::mag_gradient()");
			}

			part1 = Aij * mpara->enorm[3*f+j];
			k1 = k1 - (part1 + part2);
		}

		out_grad = out_grad - 1e+9*Cm*k1*mpara->mag_amp[f];
	}

	return out_grad;
}

gctl::tensor magtet::mag_tensor(const mag_tetrahedron &tet, const gctl::point3dc &site)
{
	double Rij_minus, Rij_plus, Sij_plus, Sij_minus, Rij0, mij0, wi0;
	double beta, sig, absw;
	double factor_n_mij, factor_tij;
	gctl::point3dc oi, grad_Aij;
	gctl::point3dc grad_Rij_plus, grad_Rij_minus, grad_Sij_plus, grad_Sij_minus;
	gctl::point3dc grad_mij0, grad_Rij0, grad_abs_wi0;
	gctl::point3dc grad_a_plus, grad_b_plus, grad_a_minus, grad_b_minus;
	gctl::point3dc grad_betaij_plus, grad_betaij_minus, grad_betaij;
	double a_plus, b_plus, a_minus, b_minus;
	double k3;
	gctl::tensor tmp_k, k2;

	// get attribute pointer
	magtet_para *mpara = tet.att;

	gctl::tensor out_tensor(0.0);
	for (int f = 0; f < 4; ++f)
	{
		k2.set(0.0);
		k3 = 0.0;
		for (int j = 0; j < 3; ++j)
		{
			Rij_minus = (site - *tet.fget(f, j)).module();
			Rij_plus  = (site - *tet.fget(f, (j+1)%3)).module();

			if (j == 0)
			{
				wi0 = gctl::dot(site - *tet.fget(f, j), mpara->fnorm[f]);
				sig = gctl::sign(wi0);
				absw = std::abs(wi0);
			}

			oi = site - wi0*mpara->fnorm[f];
			Sij_minus = gctl::dot(*tet.fget(f, j) - oi, mpara->etang[3*f+j]);
			Sij_plus  = gctl::dot(*tet.fget(f, (j+1)%3) - oi, mpara->etang[3*f+j]);
			mij0 = gctl::dot(*tet.fget(f, j) - oi, mpara->enorm[3*f+j]);
			Rij0 = std::sqrt(wi0*wi0 + mij0*mij0);

			if (std::abs(Rij0) > TOL)
			{
				factor_n_mij = -1.0*(Sij_plus/(Rij0*Rij0*Rij_plus) - Sij_minus/(Rij0*Rij0*Rij_minus));
				factor_tij   = -1.0/Rij_plus + 1.0/Rij_minus;
				grad_Aij = (wi0*factor_n_mij)*mpara->fnorm[f] + factor_tij*mpara->etang[3*f+j] 
					- (mij0*factor_n_mij)*mpara->enorm[3*f+j];
			}
			else
			{
				factor_tij   = -1.0/Rij_plus + 1.0/Rij_minus;
				grad_Aij = factor_tij*mpara->etang[3*f+j];
			}

			//tmp_k = gctl::kron(grad_Aij, enorm_[e][3*f+j]);
			tmp_k = gctl::kron(mpara->enorm[3*f+j], grad_Aij);
			k2 = k2 - tmp_k;

			if (absw > TOL)
			{
				grad_Rij_plus  = (1.0/Rij_plus)*(site - *tet.fget(f, (j+1)%3));
				grad_Rij_minus = (1.0/Rij_minus)*(site - *tet.fget(f, j));
				grad_Sij_plus  = -1.0*mpara->etang[3*f+j];
				grad_Sij_minus = -1.0*mpara->etang[3*f+j];
				grad_mij0    = -1.0*mpara->enorm[3*f+j];
				grad_Rij0    = (1.0/Rij0)*(wi0*mpara->fnorm[f] - mij0*mpara->enorm[3*f+j]);
				grad_abs_wi0 = sig*mpara->fnorm[f];
				a_plus = Rij0*Rij0 + absw*Rij_plus;
				b_plus = mij0*Sij_plus;
				grad_a_plus = (2.0*Rij0)*grad_Rij0 + Rij_plus*grad_abs_wi0 + absw*grad_Rij_plus;
				grad_b_plus = Sij_plus*grad_mij0 + mij0*grad_Sij_plus;
				a_minus = Rij0*Rij0 + absw*Rij_minus;
				b_minus = mij0*Sij_minus;
				grad_a_minus = (2.0*Rij0)*grad_Rij0 + Rij_minus*grad_abs_wi0 + absw*grad_Rij_minus;
				grad_b_minus = Sij_minus*grad_mij0 + mij0*grad_Sij_minus;
				grad_betaij_plus  = (1.0/(a_plus*a_plus + b_plus*b_plus))*(a_plus*grad_b_plus - b_plus*grad_a_plus);
				grad_betaij_minus = (1.0/(a_minus*a_minus + b_minus*b_minus))*(a_minus*grad_b_minus - b_minus*grad_a_minus);

				grad_betaij = grad_betaij_plus - grad_betaij_minus;

				tmp_k = gctl::kron(grad_betaij, mpara->fnorm[f]);
				k2 = k2 - sig*tmp_k;
			}
			else
			{
				if (std::abs(mij0) > TOL)
				{
					k3 += (-1.0/mij0)*(Sij_plus/Rij_plus - Sij_minus/Rij_minus);
				}
			}
		}

		if (k3 != 0.0)
		{
			tmp_k = gctl::kron(mpara->fnorm[f], mpara->fnorm[f]);
			k2 = k2 - k3*tmp_k;
		}

		out_tensor = out_tensor - 1e+9*Cm*k2*mpara->mag_amp[f];
	}

	return out_tensor;
}

void magtet::total_potential()
{
	int e, s;
	// malloc space
	mag_pot_.resize(site_num_, 0.0);

	gctl::progress_bar bar(ele_num_, "MagPotential");
	for (e = 0; e < ele_num_; ++e)
	{
		bar.progressed(e);

#pragma omp parallel for private (s) schedule (guided)
		for (s = 0; s < site_num_; ++s)
		{
			mag_pot_[s] += mag_potential(ele_[e], site_[s]);
		}
	}
	return;
}

void magtet::total_gradient()
{
	int e, s;
	// malloc space
	mag_grad_.resize(site_num_, gctl::point3dc(0.0, 0.0, 0.0));

    gctl::progress_bar bar(ele_num_, "MagGradient");
	for (e = 0; e < ele_num_; ++e)
	{
		bar.progressed(e);

#pragma omp parallel for private (s) schedule (guided)
		for (s = 0; s < site_num_; ++s)
		{
			mag_grad_[s] = mag_grad_[s] + mag_gradient(ele_[e], site_[s]);
		}
	}
	return;
}

void magtet::total_tensor()
{
	int e, s;
	// malloc space
	mag_tensor_.resize(site_num_, gctl::tensor(0.0));

    gctl::progress_bar bar(ele_num_, "MagTensor");
	for (e = 0; e < ele_num_; ++e)
	{
		bar.progressed(e);

#pragma omp parallel for private (s) schedule (guided)
		for (s = 0; s < site_num_; ++s)
		{
			mag_tensor_[s] = mag_tensor_[s] + mag_tensor(ele_[e], site_[s]);
		}
	}
	return;
}

//double magtet::sign(double s)
//{
//	if (s > TOL) return 1.0;
//	if (s < -1.0*TOL) return -1.0; 
//	return 0.0;
//}

void magtet::routine(const char *para_file)
{
	// initialize options
	gopt_.init_options(para_file);
	gopt_.show_options();

	// read tetgen files
	RUN_ECHO(read_tet(gopt_.get_value(TETFILE)), "Reading 3D model file");
	// read magnetization file
	RUN_ECHO(init_magz(gopt_.get_value(MAGFILE)), "Reading magnetization file");
	// read site file
	RUN_ECHO(init_site(gopt_.get_value(SITEFILE)), "Initiating observations points");
	// initialize tensors
	RUN_ECHO(cal_tensors(), "Calculating tensors");

	// calculate targets
	bool matched = false;
	std::string target_str = gopt_.get_value(CALTYPE);

	int pos = target_str.find("potential");
	if (pos != target_str.npos)
	{
		RUN_ECHO(total_potential(), "Calculating magnetic potential");
		matched = true;
	}

	pos = target_str.find("gradient");
	if (pos != target_str.npos)
	{
		RUN_ECHO(total_gradient(), "Calculating magnetic gradient");
		matched = true;
	}

	pos = target_str.find("tensor");
	if (pos != target_str.npos)
	{
		RUN_ECHO(total_tensor(), "Calculating magnetic tensor");
		matched = true;
	}

	if (!matched)
	{
		throw gctl::runtime_error("Invalid calculating type. From magtet::routine(...)");
	}

	// output results
	RUN_ECHO(write_text(gopt_.get_value(OBSFILE)), "Writing outputs");
	return;
}

// Here starts the main function
int main(int argc, char const *argv[])
{
	if (argc == 1)
	{
		std::clog << "Magnetic gradient tensors of homologous tetrahedrons.\n";
		std::clog << "Usage: ./magtet <para-file>\n";
		std::clog << "Option format:\n";
		std::clog << "<option> = <value>\n";
		std::clog << "Available options:\n";
		std::clog << "tet-file: 3D model file. this option needs both .node and .ele files generated by Tetgen. No file extension is needed\n";
		std::clog << "mag-file: magnetization vectors. format1: <filename> format2: (<magz-x>,<magz-y>,<magz-z>)\n";
		std::clog << "mag-file: line format: <magz-x> <magz-y> <magz-z>\n";
		std::clog << "site-file: observation points. format1: <filename> format2: <xmin>/<xmax>/<ymin>/<ymax>/<z>/<xnum>/<ynum>\n";
		std::clog << "site-file: line format: <x> <y> <z>\n";
		std::clog << "obs-file: output observation files. No file extension is needed\n";
		std::clog << "cal-type: available calculating types: potential, gradient and tensor\n";
		return -1;
	}

	try
	{
		// declare a new object
		magtet mt;
		mt.routine(argv[1]);
		return 0;
	}
	catch(std::exception &e)
	{
		GCTL_ShowWhatError(e.what(), GCTL_ERROR_ERROR, 0, 0, 0);
	}
}
